//
// Created by Ylint on 2024/4/4.
//

#include "kalman.h"

//static float Q_angle = 0.001;		//角度数据置信度，角度噪声的协方差
//static float Q_gyro  = 0.003;		//角速度数据置信度，角速度噪声的协方差
//static float R_angle = 0.5;			//加速度计测量噪声的协方差
//static float dt      = 0.005;		//采样周期即计算任务周期10ms
//static float Q_bias;				//Q_bias:陀螺仪的偏差
//static float K_0, K_1;				//卡尔曼增益  K_0:用于计算最优估计值  K_1:用于计算最优估计值的偏差
//static float PP[2][2] = { { 1, 0 },{ 0, 1 } };//过程协方差矩阵P，初始值为单位阵
//
//float roll_kalman;
//
//
//void Kalman_Cal_Roll(float acc,float gyro) //卡尔曼滤波roll轴计算
//{
//    static float Q_bias;	//Q_bias:陀螺仪的偏差  Angle_err:角度偏量
//    static float K_0, K_1;	//卡尔曼增益  K_0:用于计算最优估计值  K_1:用于计算最优估计值的偏差 t_0/1:中间变量
//    static float PP[2][2] = { { 1, 0 },{ 0, 1 } };//过程协方差矩阵P，初始值为单位阵
//    roll_kalman += (gyro - Q_bias) * dt; //状态方程,角度值等于上次最优角度加角速度减零漂后积分
//    PP[0][0] = PP[0][0] + Q_angle - (PP[0][1] + PP[1][0])*dt;
//    PP[0][1] = PP[0][1] - PP[1][1]*dt;
//    PP[1][0] = PP[1][0] - PP[1][1]*dt;
//    PP[1][1] = PP[1][1] + Q_gyro;
//    K_0 = PP[0][0] / (PP[0][0] + R_angle);
//    K_1 = PP[1][0] / (PP[0][0] + R_angle);
//    roll_kalman = roll_kalman + K_0 * (acc - roll_kalman);
//    Q_bias = Q_bias + K_1 * (acc - roll_kalman);
//    PP[0][0] = PP[0][0] - K_0 * PP[0][0];
//    PP[0][1] = PP[0][1] - K_0 * PP[0][1];
//    PP[1][0] = PP[1][0] - K_1 * PP[0][0];
//    PP[1][1] = PP[1][1] - K_1 * PP[0][1];
//}
//


/**
 * Q 参数表示系统在预测过程中，对于模型预测值的信任程度。如果 Q 较大，表示对系统模型的预测不够自信，会导致滤波器更倾向于相信测量值；
 * 反之，如果 Q 较小，则表示对系统模型的预测更为自信，滤波器更倾向于相信模型的预测值。
 * R 参数表示系统对于测量值的信任程度。如果 R 较大，表示对测量值的不确定性较高，滤波器更倾向于相信系统模型的预测值；反之，如果 R
 * 较小，则表示对测量值的不确定性较低，滤波器更倾向于相信测量值。
 */
float Kalman_Q = 0.01;
float Kalman_R = 0.1;


KalmanFilter kf;
// 初始化卡尔曼滤波器
void kalman_init(KalmanFilter *kf, float initial_estimate, float initial_error_covariance) {
    kf->x_hat = initial_estimate;
    kf->P = initial_error_covariance;
    kf->x_hat_prev = initial_estimate;
    kf->P_prev = initial_error_covariance;
}

// 更新卡尔曼滤波器状态
void kalman_update(KalmanFilter *kf, float z, float dt) {
    // 预测步骤
    float x_hat_minus = kf->x_hat;
    float P_minus = kf->P + Kalman_Q;

    // 更新步骤
    kf->K = P_minus / (P_minus + Kalman_R);
    kf->x_hat = x_hat_minus + kf->K * (z - x_hat_minus);
    kf->P = (1 - kf->K) * P_minus;

    // 二阶卡尔曼滤波器需要考虑上一时刻的状态
    float x_hat_delta = kf->x_hat - kf->x_hat_prev;
    kf->x_hat_prev = kf->x_hat;
    kf->P_prev = kf->P;

    // 根据周期更新增益
    float alpha = dt / (dt + kf->P_prev);
    kf->K = kf->P * alpha / Kalman_R;
    kf->x_hat += kf->K * x_hat_delta;
    kf->P *= (1 - alpha);
}



